//
//  NormalizeImage.cpp
//  TestOpenCV
//
//  Created by CityDo on 2019/8/23.
//  Copyright © 2019 CityDo. All rights reserved.
//

#include "NormalizeImage.hpp"
#import <Foundation/Foundation.h>
#import <UIKit/UIKit.h>
#import "UIImage+OpenCV.h"
#import "OcObj.h"
#import "OpenCVUtil.h"
#import "QYRectView.h"
#import <opencv2/imgcodecs/ios.h>

const int IMGSIZE = 112;
const int N_LANDMARK = 68;
const int HalfSize = 8;

cv::CascadeClassifier opencvFaceDetector;
cv::Mat meanShape(N_LANDMARK, 2, CV_32F,1080);
cv::Mat meanImg(IMGSIZE, IMGSIZE, CV_32F,1080);
cv::Mat stdImg;
cv::Mat dstImg(112, 112, CV_32F,1080);
//cv::Mat dstImg = cv::Mat::zeros(112, 112, CV_32F);

NSInteger count;

//OC对象
@interface NormalizeImage : NSObject

-(void)TestLog;
-(void)saveImageToDomainsWithDirectorystringByAppendingPathComponent:(NSString *)suffix WithImage:(UIImage *)image;
-(void)saveImageToDocumentsPath:(NSString *) suffix With: (cv::Mat)mat;

@end

@implementation NormalizeImage

-(void)TestLog
{
    NSLog(@"Hello objc \n");
}

-(void)saveImageToDomainsWithDirectorystringByAppendingPathComponent:(NSString *)suffix WithImage:(UIImage *)image
{
    //JEPG格式
    //压缩图片
    NSData *imagedata=UIImageJPEGRepresentation(image,1.0);
    //获取沙盒路径
    NSArray*paths=NSSearchPathForDirectoriesInDomains(NSDocumentDirectory,NSUserDomainMask,YES);
    //获取documents路径
    NSString *documentsDirectory=[paths objectAtIndex:0];
    //添加文件名及后缀
    NSString *savedImagePath=[documentsDirectory stringByAppendingPathComponent:suffix];
    //写入文件
    [imagedata writeToFile:savedImagePath atomically:YES];
}

-(void)saveImageToDocumentsPath:(NSString *) suffix With: (cv::Mat) mat {
    //获取沙盒路径
    NSArray *paths = NSSearchPathForDirectoriesInDomains(NSDocumentDirectory, NSUserDomainMask, YES);
    //获取documents路径
    NSString *documentDir = [paths objectAtIndex:0];
    //添加文件名及后缀
    NSString *savedImagePath=[documentDir stringByAppendingPathComponent:suffix];
    
    cv::imwrite([savedImagePath UTF8String], mat);
}

@end

namespace normalize {
    
    void Image::init( std::string strDetectorFile, std::string strSettingFile){
        //        std::string strDetectorFile = pathStrOne;
        
        cv::FileStorage fread(strSettingFile.c_str(), cv::FileStorage::READ);
        fread["meanshape"] >> meanShape;
        fread["meanImg"] >> meanImg;
        fread["stdImg"] >> stdImg;
        fread.release();
        opencvFaceDetector.load(strDetectorFile);
        std::cout << "Image::init初始化成功" << std::endl;
        
    }
    
    void Image::findBbox(cv::Mat &src, std::vector<int> &Bbox,int frameIdx, cv::Mat &tempSrc,float &time)
    {
        //        cv::CascadeClassifier faceDetector("../data/haarcascade_frontalface_alt.xml");
        
        time = 0;
        if (frameIdx %10==0) {
        
            std::vector<cv::Rect> objects;
            
            double t = (double)cv::getTickCount();//开始时间
            
            opencvFaceDetector.detectMultiScale(src, objects,1.2,3,0,cv::Size(50,50));
            
            // Draw all detected faces
            for(unsigned int i = 0; i < objects.size(); i++)
            {
                const cv::Rect& face = objects[i];
                
                if (face.x!=0) {
                    t = (double)cv::getTickCount() - t;//代码运行时间=结束时间-开始时间
                    time = t*1000. / cv::getTickFrequency();
                    printf("OpenCV 找脸执行时长 = %gms\n", t*1000. / cv::getTickFrequency());//转换时间单位并输出代码运行时间
                }
            }
            
            //        for (size_t i = 0; i < box.size(); i++)
            //        {
            //            int  tempNum = box[i];
            //            std::cout <<tempNum << std::endl;
            //        }
            
            for (size_t i = 0; i < objects.size(); i++)
            {
                const cv::Rect& face  = objects[i];
                std::cout <<face << std::endl;
                
            }
            
            int maxArea = 0;
            int max_idx = -1;
            for (int i = 0; i < objects.size(); i++)
            {
                if (objects[i].area() > maxArea )
                {
                    maxArea = objects[i].area();
                    max_idx = i;
                }
            }
            
            if (max_idx > -1)
            {
                Bbox[0] = objects[max_idx].x;
                Bbox[1] = objects[max_idx].y;
                Bbox[2] = Bbox[0]+objects[max_idx].width;
                Bbox[3] = Bbox[1]+objects[max_idx].height;
            }
            
            
            // Draw all detected faces
            for(unsigned int i = 0; i < objects.size(); i++)
            {
                const cv::Rect& face = objects[i];
                // Get top-left and bottom-right corner points
                //            cv::Point tl(face.x, face.y);
                //            cv::Point br = tl + cv::Point(face.width, face.height);
                cv::Point tl( Bbox[0],  Bbox[1]);
                cv::Point br (Bbox[2],Bbox[3]);
                
                // Draw rectangle around the face
                cv::Scalar magenta = cv::Scalar(0,155,0,155);
                cv::rectangle(src, tl, br, magenta, 4, 8, 0);
                
            }
            
            NormalizeImage *nor = [NormalizeImage new];
            tempSrc = src;
            [nor saveImageToDocumentsPath:@"rectangle01.jpg" With:tempSrc];
            
        }
    }
    
    void Image::bestFitBbox(cv::Mat &fitShape, cv::Mat &meanShape, std::vector<int> &box,float &maxSize)
    {
        float centerPoint_x = float((box[0] + box[2]) / 2.0f);
        float centerPoint_y = float((box[1] + box[3]) / 2.0f);
        float boxWidth = float(box[2] - box[0]);
        float boxHeight = float(box[3] - box[1]);
        
        double min_x, max_x;
        cv::minMaxLoc(meanShape.col(0), &min_x, &max_x);
        double min_y, max_y;
        cv::minMaxLoc(meanShape.col(1), &min_y, &max_y);
        
        float meanShapeWidth = float(max_x - min_x);
        float meanShapeHeight = float(max_y - min_y);
        
        maxSize = meanShapeWidth > meanShapeHeight ? meanShapeWidth : meanShapeHeight;
        
        float scaleWidth = boxWidth / meanShapeWidth;
        float scaleHeight = boxHeight / meanShapeHeight;
        float scale = (scaleWidth + scaleHeight) / 2.0f;
        
        fitShape = meanShape * scale;
        
        
        float fitCenter_x = ((max_x + min_x)*scale)/2.0f;
        float fitCenter_y = ((max_y + min_y)*scale)/2.0f;
        
        float trans_x = centerPoint_x - fitCenter_x;
        float trans_y = centerPoint_y - fitCenter_y;
        
        fitShape.col(0) = fitShape.col(0) + trans_x;
        fitShape.col(1) = fitShape.col(1) + trans_y;
        
    }
    //
    void Image::bestFitSrc2Dst(cv::Mat &dst, cv::Mat &src, cv::Mat &Rot,cv::Mat &Trans)
    {
        cv::Scalar scaledMeanDst_x = cv::mean(dst.col(0));
        cv::Scalar scaledMeanDst_y = cv::mean(dst.col(1));
        float dstmean_x = scaledMeanDst_x.val[0];
        float dstmean_y = scaledMeanDst_y.val[0];
        
        cv::Scalar scaledMeanSrc_x = cv::mean(src.col(0));
        cv::Scalar scaledMeanSrc_y = cv::mean(src.col(1));
        float srcmean_x = scaledMeanSrc_x.val[0];
        float srcmean_y = scaledMeanSrc_y.val[0];
        
        cv::Mat srcClone = src.clone();
        cv::Mat dstClone = dst.clone();
        
        srcClone.col(0) = srcClone.col(0) - srcmean_x;
        srcClone.col(1) = srcClone.col(1) - srcmean_y;
        
        dstClone.col(0) = dstClone.col(0) - dstmean_x;
        dstClone.col(1) = dstClone.col(1) - dstmean_y;
        
        double src_dot_dst = srcClone.dot(dstClone);
        double src_sq_sum = srcClone.dot(srcClone);
        
        float a = float(src_dot_dst / src_sq_sum);
        
        double srcX_dot_dstY = srcClone.col(0).dot(dstClone.col(1));
        double srcY_dot_dstY = srcClone.col(1).dot(dstClone.col(0));
        
        float b = float((srcX_dot_dstY - srcY_dot_dstY));
        b = float(b / src_sq_sum);
        
        Rot.at<float>(0, 0) = a;
        Rot.at<float>(0, 1) = b;
        Rot.at<float>(1, 0) = -b;
        Rot.at<float>(1, 1) = a;
        
        cv::Mat srcMean(1, 2, CV_32F);
        srcMean.at<float>(0, 0) = srcmean_x;
        srcMean.at<float>(0, 1) = srcmean_y;
        
        cv::Mat temp(1, 2, CV_32F);
        temp = srcMean * Rot;
        
        Trans.at<float>(0, 0) = dstmean_x - temp.at<float>(0, 0);
        Trans.at<float>(0, 1) = dstmean_y - temp.at<float>(0, 1);
    }
    //
    //
    void Image::cropImg(cv::Mat &src, cv::Mat &meanShape, cv::Mat &initshape,cv::Mat &dstImg,cv::Mat &dstInitshape,cv::Mat &dstRot,cv::Mat &dstTrans, float &maxSize)
    {
        
        float destShapeSize = 56.0f;
        cv::Mat scaledMeanShape = meanShape * (destShapeSize / maxSize);
        
        cv::Scalar scaledMeanShape_x = cv::mean(scaledMeanShape.col(0));
        cv::Scalar scaledMeanShape_y = cv::mean(scaledMeanShape.col(1));
        float mean_x = scaledMeanShape_x.val[0];
        float mean_y = scaledMeanShape_y.val[0];
        
        cv::Mat destShape = scaledMeanShape.clone();
        
        destShape.col(0) = destShape.col(0) +56.0f- mean_x;
        destShape.col(1) = destShape.col(1) +56.0f- mean_y;
        
        cv::Mat Rot(2, 2, CV_32F);
        cv::Mat Trans(1, 2, CV_32F);
        
        bestFitSrc2Dst(destShape, initshape, Rot, Trans);
        
        cv::Mat invRot(2, 2, CV_32F);
        
        cv::invert(Rot, invRot);
        cv::Mat tempTrans = Trans * (-1.0f);
        cv::Mat Trans2 = tempTrans * invRot;
        
        int img_Cx = int(src.cols/2);
        int img_Cy = int(src.rows/2);
        
        /*cv::Mat affineMat(2, 3, CV_32F);
         affineMat.at<float>(0, 0) = invRot.at<float>(0, 0);
         affineMat.at<float>(0, 1) = invRot.at<float>(0, 1);
         affineMat.at<float>(1, 0) = invRot.at<float>(1, 0);
         affineMat.at<float>(1, 1) = invRot.at<float>(1, 1);
         affineMat.at<float>(0, 2) = Trans2.at<float>(0, 1) + (1 - affineMat.at<float>(0, 0))*img_Cx - affineMat.at<float>(0, 1)*img_Cy;
         affineMat.at<float>(1, 2) = Trans2.at<float>(0, 0) + affineMat.at<float>(0, 1)*img_Cx + (1 - affineMat.at<float>(0, 0))*img_Cy;*/
        
        cv::Mat affineMat(2, 3, CV_32F);
        affineMat.at<float>(0, 0) = Rot.at<float>(0, 0);
        affineMat.at<float>(0, 1) = Rot.at<float>(0, 1);
        affineMat.at<float>(1, 0) = Rot.at<float>(1, 0);
        affineMat.at<float>(1, 1) = Rot.at<float>(1, 1);
        affineMat.at<float>(0, 2) = Trans.at<float>(0, 0) ;
        affineMat.at<float>(1, 2) = Trans.at<float>(0, 1) ;
        
        cv::Mat temp;
        cv::warpAffine(src, temp, affineMat, cv::Size(112, 112),2);
        //cv::imshow("ffff", temp);
        //cv::waitKey(-27);
        
        
        if (count ==1) {
            NormalizeImage *nor = [NormalizeImage new];
            [nor saveImageToDocumentsPath:@"tempbefore.jpg" With:temp];
            [nor saveImageToDocumentsPath:@"cropdstImgbefore.jpg" With:dstImg];
        }
        
        temp.convertTo(dstImg, CV_32F);
        
        if (count ==1) {
            
            //            UIImage *image = [UIImage imageWithCVMat:dstImg];
            //            OcObj *oco = [OcObj new];
            //            [oco saveImageToDomainsWithDirectorystringByAppendingPathComponent:@"f0009-ios.jpg" WithImage:image];//有问题
            
            NormalizeImage *nor = [NormalizeImage new];
            [nor saveImageToDocumentsPath:@"cropdstImgafter.jpg" With:dstImg];
            [nor saveImageToDocumentsPath:@"tempafter.jpg" With:temp];
            
        }
        
        //        count ++;
        
        dstInitshape = initshape * Rot;
        dstInitshape.col(0) = dstInitshape.col(0) + Trans.at<float>(0, 0);
        dstInitshape.col(1) = dstInitshape.col(1) + Trans.at<float>(0, 1);
        
        //for (int i = 0; i < 68; i++)
        //{
        // std::cout << dstInitshape.at<float>(i, 0) << "," << dstInitshape.at<float>(i, 1) << std::endl;
        //}
        
        dstRot = invRot.clone();
        dstTrans = Trans2.clone();
        
    }
    //
    //
    void Image::normImg(cv::Mat &src, cv::Mat &dstImg,cv::Mat &dstInitshape, cv::Mat &dstRot, cv::Mat &dstTrans, std::vector<int> inBbox, int frameIdx,std::vector<int> &outBox,float &time)
    {
        count ++;
        
        if (count ==1) {
            UIImage *image = [UIImage imageWithCVMat:src];
            OcObj *oco = [OcObj new];
            [oco saveImageToDomainsWithDirectorystringByAppendingPathComponent:@"f0006.jpg" WithImage:image];
        }
        
        //        std::string strSettingsFile = "../data/MatData.yaml";
        //        cv::FileStorage fread(strSettingsFile.c_str(), cv::FileStorage::READ);
        
        //        cv::Mat meanShape(68,2,CV_32F);
        //        cv::Mat meanImg(112, 112, CV_32F);
        //        cv::Mat stdImg;
        //        fread["meanshape"] >> meanShape;
        //        fread["meanImg"] >> meanImg;
        //        fread["stdImg"] >> stdImg;
        //        fread.release();
        
        int len = src.channels();
        cv::Mat meanRGB;
        if (len > 2)
        {
            cv::Mat floatSrc;
            src.convertTo(floatSrc, CV_32FC3);
            std::vector<cv::Mat> gbrChannel;
            cv::split(floatSrc, gbrChannel);
            cv::Mat sumRgb = gbrChannel[0] + gbrChannel[1] + gbrChannel[2];
            cv::Mat tempMean = 1.0f / 3.0f*sumRgb;
            tempMean.convertTo(meanRGB, CV_8UC1);
        }
        else
        {
            src.convertTo(meanRGB, CV_8UC1);
        }
        
//        std::vector<int> Bbox(4);
        cv::Mat tempSrc;
        
        findBbox(meanRGB, inBbox,frameIdx,tempSrc,time);
        cv::Mat fitShape;
        float maxSize;
        NormalizeImage *nor = [NormalizeImage new];
        
        [nor saveImageToDocumentsPath:@"rectangle1.jpg" With:tempSrc];
        
        bestFitBbox(fitShape, meanShape, inBbox, maxSize);
        outBox = inBbox;
        
        for (size_t i = 0; i < outBox.size(); i++)
        {
            int  tempNum = outBox[i];
            std::cout <<tempNum << std::endl;
            
            //            CGRect rect = CGRectMake( Bbox[0], Bbox[1], Bbox[3]-Bbox[1], Bbox[4]-Bbox[2]);
            //            cv::rectangle(meanRGB, rect, rect, -1);
            //            QYRectView *rectView = [[QYRectView alloc]initWithFrame:rect];
            //            cv::Mat::rectangle(meanRGB,rect,Scalar(0,0,255),1,1,0);
        }
        
        cv::Mat temp(112, 112, CV_32F);
        
        if (count ==1) {
            NormalizeImage *nor = [NormalizeImage new];
            [nor saveImageToDocumentsPath:@"dstImg.jpg" With:dstImg];
            [nor saveImageToDocumentsPath:@"fitShape.jpg" With:fitShape];
            [nor saveImageToDocumentsPath:@"meanShape.jpg" With:meanShape];
            [nor saveImageToDocumentsPath:@"meanRGB.jpg" With:meanRGB];
        }
        
        cropImg(meanRGB, meanShape,fitShape, temp, dstInitshape, dstRot, dstTrans, maxSize);
        
        cv::Mat temp1 = temp - meanImg;
        dstImg= temp1/ stdImg;
        
        if (count ==1) {
            
            UIImage *image = [UIImage imageWithCVMat:dstImg];
            OcObj *oco = [OcObj new];
            [oco saveImageToDomainsWithDirectorystringByAppendingPathComponent:@"f0008.jpg" WithImage:image];
            //            OpenCVUtil *openCVUtil = [OpenCVUtil new];
            //            [openCVUtil saveImageToDocumentsPath:@"f0008.jpg" With:dstImg];
            NormalizeImage *nor = [NormalizeImage new];
            [nor saveImageToDocumentsPath:@"dstImg.jpg" With:dstImg];
            [nor saveImageToDocumentsPath:@"meanImg.jpg" With:meanImg];
            [nor saveImageToDocumentsPath:@"stdImg.jpg" With:stdImg];
            [nor saveImageToDocumentsPath:@"temp1.jpg" With:temp1];
            
            //        NormalizeImage *nor = [NormalizeImage new];
        }
        
        dstImg.step[0] = src.step[0];
        [nor saveImageToDocumentsPath:@"rectangle2.jpg" With:tempSrc];
        
    }
    
    void Image::affineLandmarks(float(&src)[136], int(&dst)[136], cv::Mat &Rot, cv::Mat &Trans)
    {
        cv::Mat srcLandmarks(68, 2, CV_32F, src);
        cv::Mat dstLandmarks = srcLandmarks * Rot;
        dstLandmarks.col(0) = dstLandmarks.col(0) + Trans.at<float>(0, 0);
        dstLandmarks.col(1) = dstLandmarks.col(1) + Trans.at<float>(0, 1);
        
        for (int i = 0; i < 68; i++)
        {
            dst[2 * i] = int(dstLandmarks.at<float>(i, 0));
            std::cout <<"NormalizeImage.hpp中的点位值是" <<dst[2 * i] << std::endl;
            dst[2 * i + 1] = int(dstLandmarks.at<float>(i, 1));
            std::cout << dst[2 * i+1] << std::endl;
        }
    }
    
}
